// ************************************************
// SmVR_Matrix.h
//		implements matrix operations
//
//  Author: Marcio Serolli Pinho
//	Email: pinho@inf.pucrs.br
//	Homepages:
//		http://www.smallvr.org
//		http://grv.inf.pucrs.br
//		http://www.inf.pucrs.br/~pinho
//
//	Porto Alegre, Brazil, 2002.
// ************************************************

#include <stdio.h>
#include <math.h>

#ifdef WIN32
#include <memory.h>
#endif

#include <gl/gl.h>
#include "SmVR_Matrix.h"

SmVR_Matrix MatObj, MatApont;

// ************************************************
// void SetIdentityMatrix(SmVR_Matrix M)
//
//
//
// ************************************************
void SmVR_SetIdentityMatrix(SmVR_Matrix M)
{
	int i,j;

	for(i=0; i<4; i++)
		for(j=0; j<4; j++)
			if (i==j)
				M[i][j] = 1;
			else M[i][j] = 0;
}
// ************************************************
// void SmVR_CopyMatrix(SmVR_Matrix dest, SmVR_Matrix source)
//
//
//
// ************************************************
void SmVR_CopyMatrix(SmVR_Matrix dest, SmVR_Matrix source)
{
	memcpy(dest, source, sizeof (SmVR_Matrix));
}
// ************************************************
// void SmVR_MultMatrix2(SmVR_Matrix m, SmVR_Matrix n)
//		Function for matrix multiplcation.
//		This version implements the "obvious"
//	loop version of multiplication algorithm.
//	It is very slow compared with the
// ************************************************
void SmVR_MultMatrix3(SmVR_Matrix m, SmVR_Matrix n, SmVR_Matrix t)
{// M = M * N
  int		i, j, k;

  for (i=0; i<4; i++)
    for (j=0; j<4; j++)
	{
	  //printf("t[%d][%d] = ", i,j);
      for (k=0, t[i][j]=0.0; k<4; k++)
      {
		  t[i][j] += m[i][k]*n[k][j];
		  //printf("m[%d][%d] * n[%d][%d] +",i,k,k,j);
	  }
	  //printf(";\n");
	}
  //memcpy(m,t,sizeof(SmVR_Matrix));
}


// ************************************************
// void SmVR_MultMatrix2(SmVR_Matrix m, SmVR_Matrix n, SmVR_Matrix mr)
//		Function for matrix multiplcation.
//		This version implements all the
//	multiplications by "hand"
// ************************************************
void SmVR_MultMatrix2(SmVR_Matrix m, SmVR_Matrix n, SmVR_Matrix mr)
{
	mr[0][0] = m[0][0] * n[0][0] + m[0][1] * n[1][0] + m[0][2] * n[2][0] + m[0][3] * n[3][0] ;
	mr[0][1] = m[0][0] * n[0][1] + m[0][1] * n[1][1] + m[0][2] * n[2][1] + m[0][3] * n[3][1] ;
	mr[0][2] = m[0][0] * n[0][2] + m[0][1] * n[1][2] + m[0][2] * n[2][2] + m[0][3] * n[3][2] ;
	mr[0][3] = m[0][0] * n[0][3] + m[0][1] * n[1][3] + m[0][2] * n[2][3] + m[0][3] * n[3][3] ;
	mr[1][0] = m[1][0] * n[0][0] + m[1][1] * n[1][0] + m[1][2] * n[2][0] + m[1][3] * n[3][0] ;
	mr[1][1] = m[1][0] * n[0][1] + m[1][1] * n[1][1] + m[1][2] * n[2][1] + m[1][3] * n[3][1] ;
	mr[1][2] = m[1][0] * n[0][2] + m[1][1] * n[1][2] + m[1][2] * n[2][2] + m[1][3] * n[3][2] ;
	mr[1][3] = m[1][0] * n[0][3] + m[1][1] * n[1][3] + m[1][2] * n[2][3] + m[1][3] * n[3][3] ;
	mr[2][0] = m[2][0] * n[0][0] + m[2][1] * n[1][0] + m[2][2] * n[2][0] + m[2][3] * n[3][0] ;
	mr[2][1] = m[2][0] * n[0][1] + m[2][1] * n[1][1] + m[2][2] * n[2][1] + m[2][3] * n[3][1] ;
	mr[2][2] = m[2][0] * n[0][2] + m[2][1] * n[1][2] + m[2][2] * n[2][2] + m[2][3] * n[3][2] ;
	mr[2][3] = m[2][0] * n[0][3] + m[2][1] * n[1][3] + m[2][2] * n[2][3] + m[2][3] * n[3][3] ;
	mr[3][0] = m[3][0] * n[0][0] + m[3][1] * n[1][0] + m[3][2] * n[2][0] + m[3][3] * n[3][0] ;
	mr[3][1] = m[3][0] * n[0][1] + m[3][1] * n[1][1] + m[3][2] * n[2][1] + m[3][3] * n[3][1] ;
	mr[3][2] = m[3][0] * n[0][2] + m[3][1] * n[1][2] + m[3][2] * n[2][2] + m[3][3] * n[3][2] ;
	mr[3][3] = m[3][0] * n[0][3] + m[3][1] * n[1][3] + m[3][2] * n[2][3] + m[3][3] * n[3][3] ;
}
// ************************************************
// void SmVR_MultMatrix21(SmVR_Matrix m, SmVR_Matrix n, SmVR_Matrix mr)
//		Function for matrix multiplcation.
//		This version implements all the
//	multiplications by "hand" considering that the
// collumn 3 is allways :0 0 0 1
// ************************************************
void SmVR_MultMatrix21(SmVR_Matrix m, SmVR_Matrix n, SmVR_Matrix mr)
{
	mr[0][0] = m[0][0] * n[0][0] + m[0][1] * n[1][0] + m[0][2] * n[2][0]  ;
	mr[0][1] = m[0][0] * n[0][1] + m[0][1] * n[1][1] + m[0][2] * n[2][1]  ;
	mr[0][2] = m[0][0] * n[0][2] + m[0][1] * n[1][2] + m[0][2] * n[2][2]  ;
	mr[0][3] =  0 ;
	mr[1][0] = m[1][0] * n[0][0] + m[1][1] * n[1][0] + m[1][2] * n[2][0]  ;
	mr[1][1] = m[1][0] * n[0][1] + m[1][1] * n[1][1] + m[1][2] * n[2][1]  ;
	mr[1][2] = m[1][0] * n[0][2] + m[1][1] * n[1][2] + m[1][2] * n[2][2]  ;
	mr[1][3] = 0 ;
	mr[2][0] = m[2][0] * n[0][0] + m[2][1] * n[1][0] + m[2][2] * n[2][0] ;
	mr[2][1] = m[2][0] * n[0][1] + m[2][1] * n[1][1] + m[2][2] * n[2][1]  ;
	mr[2][2] = m[2][0] * n[0][2] + m[2][1] * n[1][2] + m[2][2] * n[2][2]  ;
	mr[2][3] = 0;
	mr[3][0] = m[3][0] * n[0][0] + m[3][1] * n[1][0] + m[3][2] * n[2][0] + m[3][3] * n[3][0] ;
	mr[3][1] = m[3][0] * n[0][1] + m[3][1] * n[1][1] + m[3][2] * n[2][1] + m[3][3] * n[3][1] ;
	mr[3][2] = m[3][0] * n[0][2] + m[3][1] * n[1][2] + m[3][2] * n[2][2] + m[3][3] * n[3][2] ;
	mr[3][3] = 0 ;
}

// ************************************************
// void SmVR_MultMatrix(SmVR_Matrix m1, SmVR_Matrix m2, SmVR_Matrix mr)
//		Function for matrix multiplcation.
//		This version uses the OpenGL matrix
//	functions to perform the miltiplication.
//		Tests has shown that this version spend
//	50% of the time spent by the "by hand version"
//	implemented above.
//	This tests were made in a Intel Pentium III 1GHZ
//	with a GeForce2Go video board,using Windows XP
//	and MS VC 6.0 compiler.
// ************************************************
void SmVR_MultMatrix(SmVR_Matrix m1, SmVR_Matrix m2, SmVR_Matrix mr)
{
	GLint MatrizMode;

//	MatrizMode = GL_PROJECTION_MATRIX;
//	MatrizMode = GL_MODELVIEW_MATRIX;
//	MatrizMode = GL_MODELVIEW;
//	MatrizMode = GL_PROJECTION;

	glGetIntegerv(GL_MATRIX_MODE, &MatrizMode);

	// MR = M1 * M2
	glPushMatrix();
		// set the OpenGL matrix with m1 matrix
		glLoadMatrixf(&m2[0][0]);
		// apply the rotation in the OpenGL matrix
		glMultMatrixf(&m1[0][0]);
		// set the object matrix with the new OpenGL matrix
		switch (MatrizMode)
		{
		case GL_MODELVIEW:
			glGetFloatv(GL_MODELVIEW_MATRIX, &mr[0][0]);
			break;
		case GL_PROJECTION:
			glGetFloatv(GL_PROJECTION_MATRIX, &mr[0][0]);
			break;
		case GL_TEXTURE:
			glGetFloatv(GL_TEXTURE_MATRIX, &mr[0][0]);
			break;
		default : printf("ERROR : Matrix multiplication method - OpenGL library Not Loaded !");
		}
	glPopMatrix();


}


// ************************************************
// void SmVR_InvertMatrix( float *mat, float *dst)
//		Function for matrix inversion
//		This function was copied from the document
// "Streaming SIMD Extensions - Inverse of 4x4
//	Matrix" available at the Intel site.
//
// ************************************************
void SmVR_InvertMatrix( float *mat, float *dst)
{
	float tmp[12]; /* temp array for pairs */
	float src[16]; /* array of transpose source matrix */
	float det; /* determinant */
	/* transpose matrix */
	for ( int i = 0; i < 4; i++) {
		src[i] = mat[i*4];
		src[i + 4] = mat[i*4 + 1];
		src[i + 8] = mat[i*4 + 2];
		src[i + 12] = mat[i*4 + 3];
	}
	/* calculate pairs for first 8 elements (cofactors) */
	tmp[0] = src[10] * src[15];
	tmp[1] = src[11] * src[14];
	tmp[2] = src[9] * src[15];
	tmp[3] = src[11] * src[13];
	tmp[4] = src[9] * src[14];
	tmp[5] = src[10] * src[13];
	tmp[6] = src[8] * src[15];
	tmp[7] = src[11] * src[12];
	tmp[8] = src[8] * src[14];
	tmp[9] = src[10] * src[12];
	tmp[10] = src[8] * src[13];
	tmp[11] = src[9] * src[12];
	/* calculate first 8 elements (cofactors) */
	dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7];
	dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7];
	dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7];
	dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7];
	dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7];
	dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7];
	dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6];
	dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6];
	dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3];
	dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3];
	dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3];
	dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3];
	dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3];
	dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3];
	dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2];
	dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2];
	/* calculate pairs for second 8 elements (cofactors) */
	tmp[0] = src[2]*src[7];
	tmp[1] = src[3]*src[6];
	tmp[2] = src[1]*src[7];
	tmp[3] = src[3]*src[5];
	tmp[4] = src[1]*src[6];
	tmp[5] = src[2]*src[5];
	tmp[6] = src[0]*src[7];
	tmp[7] = src[3]*src[4];
	tmp[8] = src[0]*src[6];
	tmp[9] = src[2]*src[4];
	tmp[10] = src[0]*src[5];
	tmp[11] = src[1]*src[4];
	/* calculate second 8 elements (cofactors) */
	dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15];
	dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15];
	dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15];
	dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15];
	dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15];
	dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15];
	dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14];
	dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14];
	dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9];
	dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10];
	dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10];
	dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8];
	dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8];
	dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9];
	dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9];
	dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8];
	/* calculate determinant */
	det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3];
	/* calculate matrix inverse */
	det = 1/det;
	for ( int j = 0; j < 16; j++)
		dst[j] *= det;
}

// ************************************************
// void SmVR_InvertMatrix2( float b[][4], float a[][4])
//
//		Function for matrix inversion
//		This function was copied from the document
// "Streaming SIMD Extensions - Inverse of 4x4
//	Matrix" available at the Intel site.
//
// ************************************************
void SmVR_InvertMatrix2( float b[][4], float a[][4])
{
	long indxc[4], indxr[4], ipiv[4];
	long i, icol, irow, j, ir, ic;
	float big, dum, pivinv, temp, bb;
	ipiv[0] = -1;
	ipiv[1] = -1;
	ipiv[2] = -1;
	ipiv[3] = -1;
	a[0][0] = b[0][0];
	a[1][0] = b[1][0];
	a[2][0] = b[2][0];
	a[3][0] = b[3][0];
	a[0][1] = b[0][1];
	a[1][1] = b[1][1];
	a[2][1] = b[2][1];
	a[3][1] = b[3][1];
	a[0][2] = b[0][2];
	a[1][2] = b[1][2];
	a[2][2] = b[2][2];
	a[3][2] = b[3][2];
	a[0][3] = b[0][3];
	a[1][3] = b[1][3];
	a[2][3] = b[2][3];
	a[3][3] = b[3][3];
	for (i = 0; i < 4; i++)
	{
		big = 0.0f;
		for (j = 0; j < 4; j++) {
			if (ipiv[j] != 0) {
				if (ipiv[0] == -1) {
					if ((bb = ( float) fabs(a[j][0])) > big) {
						big = bb;
						irow = j;
						icol = 0;
					}
				} else if (ipiv[0] > 0) {
					return;
				}
				if (ipiv[1] == -1) {
					if ((bb = ( float) fabs(( float) a[j][1])) > big) {
						big = bb;
						irow = j;
						icol = 1;
					}
				} else if (ipiv[1] > 0) {
					return;
				}
				if (ipiv[2] == -1) {
					if ((bb = ( float) fabs(( float) a[j][2])) > big) {
						big = bb;
						irow = j;
						icol = 2;
					}
				} else if (ipiv[2] > 0) {
					return;
				}
				if (ipiv[3] == -1) {
					if ((bb = ( float) fabs(( float) a[j][3])) > big) {
						big = bb;
						irow = j;
						icol = 3;
					}
				} else if (ipiv[3] > 0) {
					return;
				}
			}
		}
		++(ipiv[icol]);
		if (irow != icol) {
			temp = a[irow][0];
			a[irow][0] = a[icol][0];
			a[icol][0] = temp;
			temp = a[irow][1];
			a[irow][1] = a[icol][1];
			a[icol][1] = temp;
			temp = a[irow][2];
			a[irow][2] = a[icol][2];
			a[icol][2] = temp;
			temp = a[irow][3];
			a[irow][3] = a[icol][3];
			a[icol][3] = temp;
		}
		indxr[i] = irow;
		indxc[i] = icol;
		if (a[icol][icol] == 0.0) {
			return;
		}
		pivinv = 1.0f / a[icol][icol];
		a[icol][icol] = 1.0f;
		a[icol][0] *= pivinv;
		a[icol][1] *= pivinv;
		a[icol][2] *= pivinv;
		a[icol][3] *= pivinv;

		if (icol != 0) {
			dum = a[0][icol];
			a[0][icol] = 0.0f;
			a[0][0] -= a[icol][0] * dum;
			a[0][1] -= a[icol][1] * dum;
			a[0][2] -= a[icol][2] * dum;
			a[0][3] -= a[icol][3] * dum;
		}
		if (icol != 1) {
			dum = a[1][icol];
			a[1][icol] = 0.0f;
			a[1][0] -= a[icol][0] * dum;
			a[1][1] -= a[icol][1] * dum;
			a[1][2] -= a[icol][2] * dum;
			a[1][3] -= a[icol][3] * dum;
		}
		if (icol != 2) {
			dum = a[2][icol];
			a[2][icol] = 0.0f;
			a[2][0] -= a[icol][0] * dum;
			a[2][1] -= a[icol][1] * dum;
			a[2][2] -= a[icol][2] * dum;
			a[2][3] -= a[icol][3] * dum;
		}
		if (icol != 3) {
			dum = a[3][icol];
			a[3][icol] = 0.0f;
			a[3][0] -= a[icol][0] * dum;
			a[3][1] -= a[icol][1] * dum;
			a[3][2] -= a[icol][2] * dum;
			a[3][3] -= a[icol][3] * dum;
		}
	}
	if (indxr[3] != indxc[3]) {
		ir = indxr[3];
		ic = indxc[3];
		temp = a[0][ir];
		a[0][ir] = a[0][ic];
		a[0][ic] = temp;
		temp = a[1][ir];
		a[1][ir] = a[1][ic];
		a[1][ic] = temp;
		temp = a[2][ir];
		a[2][ir] = a[2][ic];
		a[2][ic] = temp;
		temp = a[3][ir];
		a[3][ir] = a[3][ic];
		a[3][ic] = temp;
	}
	if (indxr[2] != indxc[2]) {
		ir = indxr[2];
		ic = indxc[2];
		temp = a[0][ir];
		a[0][ir] = a[0][ic];
		a[0][ic] = temp;
		temp = a[1][ir];
		a[1][ir] = a[1][ic];
		a[1][ic] = temp;
		temp = a[2][ir];
		a[2][ir] = a[2][ic];
		a[2][ic] = temp;
		temp = a[3][ir];
		a[3][ir] = a[3][ic];
		a[3][ic] = temp;
	}
	if (indxr[1] != indxc[1]) {
		ir = indxr[1];
		ic = indxc[1];
		temp = a[0][ir];
		a[0][ir] = a[0][ic];
		a[0][ic] = temp;
		temp = a[1][ir];
		a[1][ir] = a[1][ic];
		a[1][ic] = temp;
		temp = a[2][ir];
		a[2][ir] = a[2][ic];
		a[2][ic] = temp;
		temp = a[3][ir];
		a[3][ir] = a[3][ic];
		a[3][ic] = temp;
	}
	if (indxr[0] != indxc[0]) {
		ir = indxr[0];
		ic = indxc[0];
		temp = a[0][ir];
		a[0][ir] = a[0][ic];
		a[0][ic] = temp;
		temp = a[1][ir];
		a[1][ir] = a[1][ic];
		a[1][ic] = temp;
		temp = a[2][ir];
		a[2][ir] = a[2][ic];
		a[2][ic] = temp;
		temp = a[3][ir];
		a[3][ir] = a[3][ic];
		a[3][ic] = temp;
	}
}

/****
 *
 * affine_matrix4_inverse
 *
 * Computes the inverse of a 3D affine matrix; i.e. a matrix with a dimen-
 * sionality of 4 where the right column has the entries (0, 0, 0, 1).
 *
 * This procedure treats the 4 by 4 matrix as a block matrix and
 * calculates the inverse of one submatrix for a significant perform-
 * ance improvement over a general procedure that can invert any non-
 * singular matrix:
 *          --        --          --          --
 *          |          | -1       |    -1      |
 *          | A      0 |          |   A      0 |
 *    -1    |          |          |            |
 *   M   =  |          |     =    |     -1     |
 *          | C      1 |          | -C A     1 |
 *          |          |          |            |
 *          --        --          --          --
 *
 *  where     M is a 4 by 4 matrix,
 *            A is the 3 by 3 upper left submatrix of M,
 *            C is the 1 by 3 lower left submatrix of M.
 *
 * Input:
 *   in   - 3D affine matrix
 *
 * Output:
 *   out  - inverse of 3D affine matrix
 *
 * Source: Wu, Kevin, Fast Matrix Inversion, Graphics Gems II, p. 342-350, code: p. 603-605,
 ***/


void SmVR_InvertMatrix3( float a[4][4], float b[4][4])
{

	register double    det_1;
             double    pos, neg, temp;

#define ACCUMULATE    \
    if (temp >= 0.0)  \
        pos += temp;  \
    else              \
        neg += temp;

#define PRECISION_LIMIT (1.0e-15)

    /*
     * Calculate the determinant of submatrix A and determine if the
     * the matrix is singular as limited by the double precision
     * floating-point data representation.
     */
    pos = neg = 0.0;
    temp =  a[0][0] * a[1][1] * a[2][2];
    ACCUMULATE
    temp =  a[0][1] * a[1][2] * a[2][0];
    ACCUMULATE
    temp =  a[0][2] * a[1][0] * a[2][1];
    ACCUMULATE
    temp = -a[0][2] * a[1][1] * a[2][0];
    ACCUMULATE
    temp = -a[0][1] * a[1][0] * a[2][2];
    ACCUMULATE
    temp = -a[0][0] * a[1][2] * a[2][1];
    ACCUMULATE
    det_1 = pos + neg;

    /* Is the submatrix A singular? */
    if ((det_1 == 0.0) || (fabs(det_1 / (pos - neg)) < PRECISION_LIMIT)) {

        /* Matrix M has no inverse */
        fprintf (stderr, "affine_matrix4_inverse: singular matrix\n");
    }

    else {

        /* Calculate inverse(A) = adj(A) / det(A) */
        det_1 = 1.0 / det_1;
        b[0][0] =   ( a[1][1] * a[2][2] -
                                 a[1][2] * a[2][1] )
                               * det_1;
        b[1][0] = - ( a[1][0] * a[2][2] -
                                 a[1][2] * a[2][0] )
                               * det_1;
        b[2][0] =   ( a[1][0] * a[2][1] -
                                 a[1][1] * a[2][0] )
                               * det_1;
        b[0][1] = - ( a[0][1] * a[2][2] -
                                 a[0][2] * a[2][1] )
                               * det_1;
        b[1][1] =   ( a[0][0] * a[2][2] -
                                 a[0][2] * a[2][0] )
                               * det_1;
        b[2][1] = - ( a[0][0] * a[2][1] -
                                 a[0][1] * a[2][0] )
                               * det_1;
        b[0][2] =   ( a[0][1] * a[1][2] -
                                 a[0][2] * a[1][1] )
                               * det_1;
        b[1][2] = - ( a[0][0] * a[1][2] -
                                 a[0][2] * a[1][0] )
                               * det_1;
        b[2][2] =   ( a[0][0] * a[1][1] -
                                 a[0][1] * a[1][0] )
                               * det_1;

        /* Calculate -C * inverse(A) */
        b[3][0] = - ( a[3][0] * b[0][0] +
                                 a[3][1] * b[1][0] +
                                 a[3][2] * b[2][0] );
        b[3][1] = - ( a[3][0] * b[0][1] +
                                 a[3][1] * b[1][1] +
                                 a[3][2] * b[2][1] );
        b[3][2] = - ( a[3][0] * b[0][2] +
                                 a[3][1] * b[1][2] +
                                 a[3][2] * b[2][2] );

        /* Fill in last column */
        b[0][3] = b[1][3] = b[2][3] = 0.0;
        b[3][3] = 1.0;

    }


}
